﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.VideoSurveillance;
using Emgu.Util;
using System.Runtime.InteropServices;

namespace LaserHarp
{      
    public struct BitBucket
        {
            public int Xtot;
            public int Ytot;
            public int pixelcount;

            public int avgX
            {
                get { return Xtot / pixelcount; }
            }
            public int avgY
            {
                get { return Ytot / pixelcount; }
            }
        }

    class ImageProcessing
    {
        // private Capture cap = new Capture();
        private PSEyeInterface myEye;
        public int threshold;
        public MKeyPoint[] keypoints;
        public MCvStarKeypoint[] stars;
        public SURFFeature[] features;

        private StarDetector detector;
        private MCvSURFParams SURFparams;

        private BlobDetector blobby;
        private BlobSeq oldseq;
        private BlobSeq newseq;

        Image<Bgra, Byte> tempimg = new Image<Bgra, Byte>(640, 480);
        Image<Gray, Byte> gray;
        Image<Bgr, Byte> img;

  


        public ImageProcessing()
        {
            myEye = new PSEyeInterface(CLEyeCameraColorMode.CLEYE_COLOR_RAW,
                CLEyeCameraResolution.CLEYE_VGA,
                CLEyeFrameRate.CLEYE_FRAMERATE_60);
            myEye.ImageArrivedEvent += new PSEyeInterface.ImageArrivedEventHandler(myEye_ImageArrivedEvent);

            detector = new StarDetector();
            //detector.LineThresholdBinarized = 10;
            //detector.LineThresholdProjected = 20;
            detector.SetDefaultParameters();

            SURFparams = new MCvSURFParams();
            SURFparams.hessianThreshold = 400;
            SURFparams.nOctaveLayers = 4;
            SURFparams.nOctaves = 3;

            blobby = new BlobDetector(BLOB_DETECTOR_TYPE.Simple);
            newseq = new BlobSeq();
            oldseq = new BlobSeq();
        }

        void myEye_ImageArrivedEvent(object sender, ImageArrivedEventArgs e)
        {
            try
            {
                tempimg = new Image<Bgra, Byte>(e.Width, e.Height, e.Stride, e.DataPointer);
            }
            catch (CvException exc)
            {
                Console.Write(exc.Message);
            }
        }

        public Point[] returnStars(int threshold)
        {
            Point[] points = null;
            detector.ResponseThreshold = threshold;
            detector.MaxSize = 6;
            stars = gray.GetStarKeypoints(ref detector);
            if (stars.Length > 0)
            {
                points = new Point[keypoints.Length];
                int count = 0;
                foreach (MCvStarKeypoint star in stars)
                {
                    points[count] = star.pt;
                    count++;
                }
            }
            return points;
        }

        public Point[] returnKeypoints(int threshold)
        {
            Point[] points = null;
            keypoints = gray.GetFASTKeypoints(threshold, true);
            if (keypoints.Length > 0)
            {
                points = new Point[keypoints.Length];
                int count = 0;
                foreach (MKeyPoint kpt in keypoints)
                {
                    points[count] = Point.Truncate(kpt.Point);
                    count++;
                }
            }
            return points;
        }

        public Point[] returnSURF(int threshold)
        {
            Point[] points = null;
            SURFparams.hessianThreshold = threshold * 2;
            features = gray.ExtractSURF(ref SURFparams);
            if (features.Length > 0)
            {
                points = new Point[features.Length];
                int count = 0;
                foreach (SURFFeature sf in features)
                {
                    MCvSURFPoint SURFpt = sf.Point;
                    points[count].X = (int)SURFpt.pt.X;
                    points[count].Y = (int)SURFpt.pt.Y;
                    count++;
                }
            }
            return points;
        }

        public Point[] returnCircles(int threshold)
        {
            Point[] points = null;
            Gray cannyThreshold = new Gray(threshold);
            Gray accThreshold = new Gray(5);
            CircleF[][] circles = gray.HoughCircles(cannyThreshold, accThreshold, 1, 30, 2, 20);

            if (circles[0].Length > 0)
            {
                points = new Point[circles[0].Length];
                int count = 0;
                foreach (CircleF circ in circles[0])
                {
                    points[count] = Point.Truncate(circ.Center);
                    count++;
                }
            }
            return points;
        }

        public List<BitBucket> returnBlobs(int threshold)
        {
            List<BitBucket> blobs = new List<BitBucket>();
            int maxsep = 20;

            //Scan image pixels
            for (int x = 0; x < 480; x++)
            {
                for (int y = 0; y < 320; y++)
                {
                    byte pixel = gray.Data[y, x, 0];
                    if (pixel == 255)
                    {
                        bool found = false;
                        for (int i = 0; i < blobs.Count; i++)
                        {
                            if (x > blobs[i].avgX - maxsep  && x < blobs[i].avgX + maxsep
                                && y > blobs[i].avgY - maxsep && y < blobs[i].avgY + maxsep)
                            {
                                // New pixel is close to existing blob so add to this blob
                                BitBucket blob = blobs[i];
                                blob.pixelcount++;
                                blob.Xtot += x;
                                blob.Ytot += y;
                                blobs[i] = blob;
                                found = true;
                                break;
                            }
                        }
                        if (!found)
                        {
                            //No match, so create new blob
                            BitBucket blob = new BitBucket();
                            blob.pixelcount = 1;
                            blob.Xtot = x;
                            blob.Ytot = y;
                            blobs.Add(blob);
                        }
                    }
                }
            }
            //Remove blobs with less pixels than threshold
            for (int i = 0; i < blobs.Count; i++)
                 if (blobs[i].pixelcount < threshold) blobs.Remove(blobs[i]);

            return blobs;
        }

        public Image<Bgr, Byte> returnImage()
        {
            img = tempimg.Convert<Bgr, Byte>();
            return img.Resize(480, 320, INTER.CV_INTER_CUBIC);
        }

        public Image<Gray, Byte> returnThresholdedImage()
        {
            gray = tempimg.Convert<Gray, Byte>();
            gray = gray.Resize(480, 320, INTER.CV_INTER_CUBIC).ThresholdBinary(new Gray(threshold), new Gray(255));
            return gray;
        }

        public void setCameraParameters(int autogain, int autoexposure, int autowhitebalance,
            int gain, int exposure, int wbred, int wbgreen, int wbblue)
        {
            myEye.camParams.AutoExposure = autoexposure;
            myEye.camParams.AutoGain = autogain;
            myEye.camParams.AutoWhiteBalance = autowhitebalance;
            myEye.camParams.Exposure = exposure;
            myEye.camParams.Gain = gain;
            myEye.camParams.WhiteBalanceBlue = wbblue;
            myEye.camParams.WhiteBalanceGreen = wbgreen;
            myEye.camParams.WhiteBalanceRed = wbred;

            myEye.setCamParams();
        }
    }
}
